#include "fsm_standby.h"
#include "axisgroup.h"
#include "axis/axis.h"
#include "errcode.h"
#include "log/zlog.hh"

using axisgroup::AxisGroup;
using axisgroup::FsmStandby;

#define ROB_DBG(fmt, ...) zlog()->debug("[AxisGroup{}] Standby:" fmt, grp->get_group_id(), ##__VA_ARGS__)
#define ROB_ERR(fmt, ...) zlog()->error("[AxisGroup{}] Standby:" fmt, grp->get_group_id(), ##__VA_ARGS__)

FsmStandby::FsmStandby(AxisGroup* g) : grp(g) { reset(); }
void FsmStandby::reset() {}

void FsmStandby::update(bool first)
{
    if (first)
    {
        reset();
    }

    if (grp->is_sim())
    {
        if (!grp->cmd_.enable)
        {
            grp->change_fsm_stat(AxisGroup::FsmState::Disabled);
        }
        return;
    }

    for (auto& joint : grp->joints)
    {
        if (joint->run_state() == axis::Axis::RunState::ErrorStop)
        {
            ROB_ERR("Joint {} is errorstop", joint->get_id());
            grp->change_to_errstat(RobEvCode::JOINT_ERR, true);
            return;
        }
    }

    //关闭使能指令
    if (!grp->cmd_.enable)
    {
        for (auto& joint : grp->joints) { joint->enable(false); }
        grp->change_fsm_stat(AxisGroup::FsmState::Disabled);
        return;
    }

    //检查是否有未使能
    for (auto& joint : grp->joints)
    {
        if (joint->run_state() == axis::Axis::RunState::Disable)
        {
            ROB_ERR("Joint {} is disable", joint->get_id());
            grp->change_to_errstat(RobEvCode::JOINT_DISABLE, true);
            return;
        }
    }
}
